#include "udt_interface.h"
#include <math.h>
#include <stdlib.h>
#include <memory.h>
#include <assert.h>
#include <stdio.h>

int _compare_points(const void *l, const void *r)
{
	if (((UDTPoint*)l)->value < ((UDTPoint*)r)->value)
		return -1;
	else if (((UDTPoint*)l)->value == ((UDTPoint*)r)->value)
		return 0;
	else
		return 1;
}; 


void udt_calcEndPointList(UDTIntervalSet *pIntSet, UDTPointSet* pPointSet)
{
	int i;
	pPointSet->pIntSet = pIntSet;
	pPointSet->point_count = pIntSet->interval_count * 2;	
	for (i = 0; i < pIntSet->interval_count; ++i) {
		pPointSet->points[i*2].value = pIntSet->intervals[i].low;
		pPointSet->points[i*2+1].value = pIntSet->intervals[i].high;
		pPointSet->points[i*2].flagLeft = 1;
		pPointSet->points[i*2+1].flagLeft = 0;
		pPointSet->points[i*2].pInt = pPointSet->points[i*2+1].pInt = &pIntSet->intervals[i];
	};
	qsort(pPointSet->points, pPointSet->point_count, sizeof(UDTPoint), _compare_points);		
};


double _entropy(double p, double q)
{
	assert(p >= -0.01); 
	assert(q >= -0.01);
	assert(q - p >= -0.01);
	if (fabs(p) < 0.00001 || fabs(q) < 0.000001)
		return 0;
	return -p*log(p/q);	
};


double udt_calcEntropy(UDTPointSet* pPointSet, double *splitPoint)
{
	int i, j;
	double sum, min = 1000000, left = 0, right = pPointSet->pIntSet->interval_count, N;
	double leftLabel[MAX_LABEL];
	double rightLabel[MAX_LABEL];
	double origin[MAX_LABEL];

	double dLeft = 0, delta; //delta left
	double dLeftLabel[MAX_LABEL];
	
	int label, label_count = pPointSet->pIntSet->label_count;
	double interval_size;
	double temp;

	/* 
	for (i = 99; i < 102; ++i)
		printf("%lf ", pPointSet->points[i].value);
	printf("\n");
	*/


	right = 0;
 	for (i = 0; i < pPointSet->pIntSet->interval_count; ++i) 
		right += (pPointSet->pIntSet->intervals[i].high - pPointSet->pIntSet->intervals[i].low)
			    * pPointSet->pIntSet->intervals[i].density;
	

	memset(leftLabel, 0, sizeof(double)*MAX_LABEL);	
	memset(rightLabel, 0, sizeof(double)*MAX_LABEL);
	for (i = 0; i < pPointSet->pIntSet->interval_count; ++i) {
		rightLabel[pPointSet->pIntSet->intervals[i].label] 
			+= (pPointSet->pIntSet->intervals[i].high - pPointSet->pIntSet->intervals[i].low)
			  * pPointSet->pIntSet->intervals[i].density;
		origin[pPointSet->pIntSet->intervals[i].label] = rightLabel[pPointSet->pIntSet->intervals[i].label];
	};

	memset(dLeftLabel, 0, sizeof(double)*MAX_LABEL);	
		
	for (i = 0; i < pPointSet->point_count-1; ++i) {
		label = pPointSet->points[i].pInt->label;
		interval_size = pPointSet->points[i+1].value - pPointSet->points[i].value;
		sum = 0;
		
		for (j = 0; j < label_count; ++j) 					
			sum += _entropy(leftLabel[j], left) + _entropy(rightLabel[j], right);
		assert(sum >= -0.0001);
		if (sum < min) {
			min = sum;
			*splitPoint = pPointSet->points[i].value;
		};

		delta = pPointSet->points[i].pInt->density;
		assert(delta > 0);
		if (!pPointSet->points[i].flagLeft) 
			delta = -delta;		
		dLeft += delta;
		dLeftLabel[label] += delta;			
		
		left += dLeft * interval_size ;
		for (j = 0; j < pPointSet->pIntSet->label_count; ++j)
			leftLabel[j] += dLeftLabel[j] * interval_size;							

		assert(left - leftLabel[label] >= -0.01);
		assert(leftLabel[label] > -0.001);
		assert(origin[label] - leftLabel[label] >= -0.01);	

		right -= dLeft * interval_size ;
		for (j = 0; j < pPointSet->pIntSet->label_count; ++j)
			rightLabel[j] -= dLeftLabel[j] * interval_size;							
		

		temp = 0;
		for (j = 0; j < MAX_LABEL; ++j)
			temp += rightLabel[j];
//		printf("temp %d %lf %lf %lf %lf\n", i, temp, right, dLeft, dLeftLabel[label]);
		assert(fabs(temp - right) <= 0.001);

		assert(rightLabel[label] > -0.001);		
		assert(fabs(leftLabel[label]+rightLabel[label]-origin[label]) <= 0.01);
		assert(right - rightLabel[label] >= -0.01);		

//		printf("right %lf %lf\n", right, rightLabel[label]);
	};				

	return min;	
};
